#!/usr/bin/env python 
import cv2
import v4l2capture as v4l2
import select
import numpy as np
import rospy
import cv2
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
import time
import signal
import sys,traceback
class CameraPub:
    def __init__(self):
        # self.TOPIC_CAMERA = "/sensors/camera"         
        self.TOPIC_CAMERA = "/sensors/camera_two"
        rospy.init_node("cameraFront")
        self.image_pub = rospy.Publisher(self.TOPIC_CAMERA, Image, queue_size=1)
        self.bridge = CvBridge()
        self.camid='/dev/videoFront'
        self.video = None

    def openCamera(self):
        try:
            self.video = v4l2.Video_device(self.camid)
            size_x, size_y = self.video.set_format(640, 480, fourcc='MJPG')
            self.video.create_buffers(10)
            self.video.queue_all_buffers()
            self.video.start()
            time.sleep(1)
        except IOError,err:
            print("cant open camere",str(err))
            time.sleep(1)
        except Exception,err:
            print(err)

    def pubCamera(self):
        if self.video is not None:
            select.select((self.video,), (), ())
            image_data = self.video.read_and_queue()
            image = np.frombuffer(image_data, dtype=np.uint8)
            frame = cv2.imdecode(image, cv2.IMREAD_COLOR)
            self.image_pub.publish(self.bridge.cv2_to_imgmsg(frame, "bgr8"))
            # cv2.imshow('view', framde)
            
        else:
            self.openCamera()
            

    def loop(self):
        signal.signal(signal.SIGINT, quit)                                
        signal.signal(signal.SIGTERM, quit)
        while(True):
            try:
                self.pubCamera()
                # if cv2.waitKey(20) & 0xFF==ord('q'):
                #     break
                time.sleep(0.03)
            except Exception,err:
                self.openCamera()
                traceback.print_exc()
                time.sleep(1)
                # break

if __name__ == "__main__":
    cpub  = CameraPub()
    cpub.loop()
